//
// Created by 86137 on 2024/1/25.
//

#include "chassis_data_calculate.h"

ctrl_msg_data my_pose={0,0,0,0,0};
ctrl_msg_data target_pose={60,80,0,0,0};
extern rc_blue_t blue_rc;
extern rc_device_t g_rc;
float wheel_rpm[4]={0};
float wheel_round[4]={0};
float wheel_last_round[4]={0};
float wheel_theta[4]={0};
float wheel_cycle[4]={0};
float last_cycle=0;
float wheel_err=0;

void chassis_data_calculate(void){
    static float vx=0;
    static float vy=0;
    static float vw=0;
    int i;
    float theta = atan(1.0/1.0);
    if(g_rc.rc_info.sw.sw_l==RC_SW_UP) {
        vy = g_rc.rc_info.rocker.left_x;//已归一化，将右边x轴摇杆值归一为-1~1
        vx = g_rc.rc_info.rocker.left_y;//已归一化，将右边x轴摇杆值归一为-1~1
        vw = g_rc.rc_info.rocker.right_x * 3.14 / 180;
        vw=vw*32;
    }
if(g_rc.is_lost==1 && blue_rc.raw_blue_data!=0)
{
    switch (blue_rc.raw_blue_data) {
        case 1:
            vx=0.7;
            vy=0;
            vw=0;
            break;
        case 2:
            vx=-0.7;
            vy=0;
            vw=0;
            break;
        case 3:
            vx=0;
            vy=-0.7;
            vw=0;
            break;
        case 4:
            vx=0;
            vy=0.7;
            vw=0;
            break;
        case 5:
            vx=0;
            vy=0;
            vw=-0.7;
            break;
        case 6:
            vx=0;
            vy=0;
            vw=0.7;
            break;
    }
}

    wheel_rpm[0]= sqrt( pow(vx - vw*sin(theta),2)+pow(vy - vw*cos(theta),2))/sqrt(2);
    wheel_rpm[1]= sqrt(	pow(vx - vw*sin(theta),2)+pow(vy + vw*cos(theta),2))/sqrt(2);
    wheel_rpm[2]= sqrt(	pow(vx + vw*sin(theta),2)+pow(vy + vw*cos(theta),2))/sqrt(2);
    wheel_rpm[3]= sqrt(	pow(vx + vw*sin(theta),2)+pow(vy - vw*cos(theta),2))/sqrt(2);//行进轮速度解算

    wheel_rpm[0]=wheel_rpm[0]/6;
    wheel_rpm[1]=wheel_rpm[1]/6;
    wheel_rpm[2]=wheel_rpm[2]/6;
    wheel_rpm[3]=wheel_rpm[3]/6;

    wheel_round[0]=atan2((vy-vw*sin(theta)),(vx-vw*cos(theta)))/6.28;
    wheel_round[1]=atan2((vy+vw*sin(theta)),(vx-vw*cos(theta)))/6.28;
    wheel_round[2]=atan2((vy+vw*sin(theta)),(vx+vw*cos(theta)))/6.28;
    wheel_round[3]=atan2((vy-vw*sin(theta)),(vx+vw*cos(theta)))/6.28;       //舵轮角度解算

    for(i=0;i<4;i++)//当算出的转角超过45度时，将转角转换到-45度~45度范围内且反转
    {
//        wheel_err=absolute(wheel_round[i]-wheel_last_round[i]);
//        if(wheel_err>0.5) wheel_err=1-wheel_err;
//        if(wheel_err>0.25){
//            if(wheel_round[i]>0){
//                wheel_round[i]=wheel_round[i]-0.5;
//            }
//            else if(wheel_round[i]<0){
//                wheel_round[i]=wheel_round[i]+0.5;
//            }
//            else if(wheel_round[i]==0){
//                if(wheel_last_round[i]<0) wheel_round[i]=-0.5;
//                else if(wheel_last_round[i]>0) wheel_round[i]=0.5;
//            }
//        }
//        else{
//            wheel_round[i]=wheel_round[i];
//        }
//    wheel_last_round[i]=wheel_round[i];
        if(wheel_round[i]>0.26)
        {
            wheel_round[i]=wheel_round[i]-0.5;
            wheel_rpm[i]=-wheel_rpm[i];
        }
        if(wheel_round[i]<-0.26)
        {
            wheel_round[i]=wheel_round[i]+0.5;
            wheel_rpm[i]=-wheel_rpm[i];
        }
    }

}


void chassis_round_calculate(void){
    int i;
    float theta=0,cycle=0;
    for(i=0;i<4;i++)
    {
        if(target_pose.y!=0)   theta=atan(abs(target_pose.y)/abs(target_pose.x))/6.2832;
        else theta=0;
        cycle=(sqrt(pow(target_pose.x,2)+pow(target_pose.y,2))*MOTOR_RATIO)/ONE_CYCLE;
        if(target_pose.x>=0 && target_pose.y>0){
            wheel_theta[i]=theta;
            wheel_cycle[i]=last_cycle+cycle;
        }
        else if(target_pose.x>=0 && target_pose.y<=0){
            wheel_theta[i]=-theta;
            wheel_cycle[i]=last_cycle+cycle;
        }
        else if(target_pose.x<0 && target_pose.y>0){
            wheel_theta[i]=-theta;
            wheel_cycle[i]=last_cycle-cycle;
        }
        else if(target_pose.x<0 && target_pose.y<=0){
            wheel_theta[i]=theta;
            wheel_cycle[i]=last_cycle-cycle;
        }
    }

}

void chassis_round_update(void){
    last_cycle=wheel_cycle[0];
}

float absolute(float x){
    if(x<0) return -x;
    else return x;
}